﻿using AGVController.BaseClass;
using System;
using System.Collections.Generic;
using System.IO;
using System.Linq;
using System.Net.Sockets;
using System.Runtime.InteropServices;
using System.Text;
using System.Threading;
using System.Threading.Tasks;

namespace AGVController
{
    public class ServiceAgvNavigate
    {

        /// <summary>
        /// 报警码编号
        /// </summary>
        private string _agvIp { get; set; }
        /// <summary>
        ///报警发生时间 
        /// </summary>
        private int _agvPort { get; set; }
        public TcpClient Client { get; set; }
        public ServiceAgvNavigate(string agvIp, int agvPort)
        {
            _agvIp = agvIp;
            _agvPort = agvPort;
            Client = new TcpClient(agvIp, agvPort); 
        }


        #region ADD BY CJH

        #region AGV路径导航流程

        //1. 机器人开机
        //2. 等待机器人启动完成（使用 TCP 连接机器人成功后即为启动完成）
        //3. 执行重定位 2002
        //4. 查询定位状态，等待定位完成1021
        //5. 确认定位正确 2003
        //6. 执行路径导航 3051
        //7. 查询导航状态，等待导航完成 1020
        //8. 循环执行步骤 6、7 即可实现持续的路径导航

        #endregion
        #region 给机器人发送指令 

        private string SendToAgv(SeerMessageHead head, byte[] data)
        {
            string strResult = "";
            try
            {
                if (Client.Connected)
                {
                    NetworkStream serverStream = Client.GetStream();

                    var newmsg = new SeerMessage();

                    newmsg.head = head;
                    newmsg.data = data;

                    #region 修改为直接发送 byt[]

                    int len = 0;
                    byte[] bHead = new byte[16];
                    bHead[len++] = 0x5A;
                    bHead[len++] = 0x01;
                    UInt16 number = head.number;
                    byte[] bNumber = BitConverter.GetBytes(number);
                    Array.Reverse(bNumber);
                    for (int i = 0; i < bNumber.Length; i++)
                    {
                        bHead[len++] = bNumber[i];
                    }
                    UInt32 length = head.length;
                    byte[] bLen = BitConverter.GetBytes(length);
                    Array.Reverse(bLen);
                    for (int i = 0; i < bLen.Length; i++)
                    {
                        bHead[len++] = bLen[i];
                    }

                    UInt16 type = head.type;
                    byte[] btype = BitConverter.GetBytes(type);
                    Array.Reverse(btype);
                    for (int i = 0; i < btype.Length; i++)
                    {
                        bHead[len++] = btype[i];
                    }

                    bHead[len++] = 0;
                    bHead[len++] = 0;
                    bHead[len++] = 0;
                    bHead[len++] = 0;
                    bHead[len++] = 0;
                    bHead[len++] = 0;

                    #endregion
                    serverStream.Write(bHead, 0, Marshal.SizeOf(newmsg.head));
                    serverStream.Write(newmsg.data, 0, newmsg.data.Length);
                    serverStream.Flush();
                    
                    SaveLog(BitConverter.ToString(bHead).Replace("-", " ") + " " + BitConverter.ToString(data).Replace("-", " ") + "="+ Encoding.UTF8.GetString(data));

                    byte[] inStream = new byte[16];
                    while (16 != serverStream.Read(inStream, 0, 16))
                    {
                        Thread.Sleep(20);
                    }

                    var recv_head = Common.bytesToStructure<SeerMessageHead>(inStream);

                    byte[] recvbyte = BitConverter.GetBytes(recv_head.length);

                    Array.Reverse(recvbyte);

                    var dsize = BitConverter.ToUInt32(recvbyte, 0);

                    const int bufferSize = 512;
                    List<byte> datalist = new List<byte>();
                    int count = 0;

                    while (true)
                    {
                        byte[] buffer = new byte[bufferSize];
                        int readSize = serverStream.Read(buffer, 0, bufferSize);

                        count += readSize;
                        datalist.AddRange(buffer);

                        if (count == dsize)
                        {
                            break;
                        }

                        Thread.Sleep(10);
                    }

                    //textBox_recv_head.Text = BitConverter.ToString(seerMessageHeadToBytes(recv_head)).Replace("-", " ");//normalStrToHexStr(Encoding.UTF8.GetString(seerMessageHeadToBytes(recv_head)));

                    string str = Encoding.UTF8.GetString(datalist.ToArray());

                    //textBox_recv_data.Invoke(new EventHandler(delegate { textBox_recv_data.Text = str; }));

                    //Client.Close();

                    strResult = str;
                }
                else
                {
                    strResult = "ConnectError";
                    Client.Close();
                    Client = new TcpClient(_agvIp, _agvPort);
                }
            }
            catch (SocketException)
            {
                //textBox_recv_data.Invoke(new EventHandler(delegate { textBox_recv_data.Text = ""; }));
                //MessageBox.Show("Connect Error!");
                strResult = "ConnectException";
            }
            catch (IOException)
            {
                //textBox_recv_data.Invoke(new EventHandler(delegate { textBox_recv_data.Text = ""; }));
                //MessageBox.Show("");

                strResult = "IOException";
            }
            return strResult;
        }


        #endregion

        #region 机器人导航

        /// <summary>
        /// 路径导航
        /// </summary>
        public string robot_task_gotarget_req(string data)
        {
            byte[] byteData = Common.normalStrToHexByte(data);
            return SendToAgv(Common.getSeerMessageHead(3051, (uint)byteData.Length), byteData);
        }


        /// <summary>
        /// 暂停当前导航
        /// </summary>
        public string robot_task_pause_req(string data)
        {
            byte[] byteData = Common.normalStrToHexByte(data);
            return SendToAgv(Common.getSeerMessageHead(3001, (uint)byteData.Length), byteData);
        }


        /// <summary>
        /// 继续当前导航
        /// </summary>
        public string robot_task_resume_req(string data)
        {
            byte[] byteData = Common.normalStrToHexByte(data);
            return SendToAgv(Common.getSeerMessageHead(3002, (uint)byteData.Length), byteData);
        }

        /// <summary>
        /// 取消当前导航
        /// </summary>
        public string robot_task_cancel_req(string data)
        {
            byte[] byteData = Common.normalStrToHexByte(data);
            return SendToAgv(Common.getSeerMessageHead(3003, (uint)byteData.Length), byteData);
        }
        #endregion
        #endregion

        private void SaveLog(string logMsg)
        {
            string path = @"C:\agvLog.txt";

            using (StreamWriter writer = new StreamWriter(path, true)) // true 表示追加模式
            {
                writer.WriteLine(logMsg);
            }
        }

    }

}
